﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO.Ports;
using WFNetLib;

namespace RobotControl
{
    public partial class Form_Main : Form
    {
        public static bool 定向 = false;
        public static bool 定深 = false;
        public static double Heading, deep;
        double HKp=5;
        double HKi=0;
        double HKd=0;
        static double[] HValue=new double[3];
        static double HOldErr;
        void HeadingPIDControl(double h)
        {
            double err = Heading - h;
            if (err < 1)
                return;
            HValue[0] = HKp * err;//比例
            HValue[1] = HValue[1] + HKi * err;//积分
            HValue[2] = HKd * (err - HOldErr);
            HOldErr = err;
            double HOut = HValue[0] + HValue[1] + HValue[2];
            SerialPort com = new SerialPort(Properties.Settings.Default.推进器通道, 9600);
            try
            {
                com.Open();
                byte[] tx = new byte[6];
                Union_UInt16 p1 = new Union_UInt16();
                p1.ofs_16 = (ushort)(推进器.k[2] * (HOut*-1) * 5 / 100 + 推进器.b[2]);
                Union_UInt16 p2 = new Union_UInt16();
                p2.ofs_16 = (ushort)(推进器.k[4] * (HOut) * 5 / 100 + 推进器.b[4]);
                tx[0] = 0xaa;
                tx[1] = 0x03;//通道号
                tx[2] = BytesOP.GetLowNibble(p1.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p1.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p1.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                tx[0] = 0xaa;
                tx[1] = 0x05;//通道号
                tx[2] = BytesOP.GetLowNibble(p2.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p2.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p2.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                com.Close();
            }
            catch { }
        }
        public static void StopHeading()
        {
            HValue[0] = 0;//比例
            HValue[1] = 0;//积分
            HValue[2] = 0;
            HOldErr = 0;
            定向 = false;
        }
    }
}
